//
// Created by jie.jiang on 2022/11/17.
//

#include "FOC_PID.h"
#include "math.h"
#include "stdint.h"
#include "time_utils.h"
#include "FOC_utils.h"

PID_DataType pid_velocity;
PID_DataType pid_angle;
uint32_t pidVelTimeStamp;
void pid_set_parameter()
{
  pid_velocity.kp = 0.1;
  pid_velocity.ki = 1;
  pid_velocity.kd = 0;
  pid_velocity.integral = 0;
  pid_velocity.last_err = 0;
  pid_velocity.max_integral = 5.0f;
  pid_velocity.min_integral = -5.0f;
  pid_velocity.max_u = 5.0f;
  pid_velocity.min_u = -5.0f;
  pid_velocity.dead_zone = 0;
  pidVelTimeStamp = DWT_CYCCNT;

  pid_angle.kp = 20.0f;
  pid_angle.ki = 0;
  pid_angle.kd = 0;
  pid_angle.integral = 0;
  pid_angle.last_err = 0;
  pid_angle.max_integral = 20.0f;
  pid_angle.min_integral = -20.0f;
  pid_angle.max_u = 20.0f;
  pid_angle.min_u = -20.0f;
  pid_angle.dead_zone = 0;



}


float pid_calc(PID_DataType *pid,float error){
  uint32_t now_us;
  float Ts;
  now_us = DWT_CYCCNT;
  Ts = (float)(DWT_CYCCNT - pidVelTimeStamp)/72 * 1e-6;
  float pTerm = pid->kp * error;
  float iTerm = pid->integral + pid->ki * Ts * 0.5f * (error + pid->last_err);
  iTerm = _constrain(iTerm,pid->min_integral,pid->max_integral);
  float dTerm =pid->kd * (error - pid->last_err) / Ts;

  float output = pTerm + iTerm + dTerm;
  output = _constrain(output,pid->min_u,pid->max_u);

  pid->last_err = error;
  pid->integral = iTerm;
  pidVelTimeStamp = now_us;

  return output;
}